import serial
import minimalmodbus
import time
import threading

class LingkongMotor:
    def __init__(self, serial_port=None, port='COM3', baudrate=115200, motor_id=0x01):
        """
        初始化电机（支持外部传入串口实例）
        :param serial_port: 已创建的串口实例（RS485共享时使用）
        :param port: 串口端口（未传入serial_port时使用）
        :param baudrate: 波特率
        :param motor_id: 电机设备地址（区分设备的关键）
        """
        # 共享串口处理
        if serial_port:
            self.ser = serial_port
        else:
            self.ser = serial.Serial(
                port=port,
                baudrate=baudrate,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                bytesize=serial.EIGHTBITS,
                timeout=0.5
            )
        self.motor_id = motor_id
        self.low_speed = 20
        self.zero_angle_target = 0

    def _calc_checksum(self, data):
        """计算校验和"""
        return sum(data) & 0xFF

    def _send_command(self, cmd, data_bytes=b''):
        """发送命令帧并返回响应"""
        frame_cmd = [0x3E, cmd, self.motor_id, len(data_bytes)]
        frame_cmd_checksum = self._calc_checksum(frame_cmd)
        frame_cmd.append(frame_cmd_checksum)
        
        full_frame = bytes(frame_cmd) + data_bytes
        if data_bytes:
            data_checksum = self._calc_checksum(data_bytes)
            full_frame += bytes([data_checksum])
            
        self.ser.write(full_frame)
        time.sleep(0.001)
        return self.ser.read(32)

    def get_current_angle(self):
        """获取当前角度（度）"""
        response = self._send_command(0x92)
        if len(response) >= 14:
            angle_bytes = response[5:13]
            current_angle = int.from_bytes(angle_bytes, byteorder='little', signed=True)
            return current_angle * 0.01  # 转换为度
        return None

    def move_to_zero(self):
        """移动到零位"""
        print("开始回归零位...")
        target_bytes = self.zero_angle_target.to_bytes(8, byteorder='little', signed=True)
        speed_value = int(self.low_speed * 100)
        speed_bytes = speed_value.to_bytes(4, byteorder='little', signed=False)
        
        self._send_command(0xA4, target_bytes + speed_bytes)
        
        start_time = time.time()
        while time.time() - start_time < 300:  # 5分钟超时
            current = self.get_current_angle()
            if current is not None and abs(current) <= 0.1:
                print("已到达零位")
                return True
            time.sleep(0.5)
        print("回零超时")
        return False

    def rotate_360(self):
        """旋转360度"""
        print("开始旋转360度...")
        target = 36000  # 360度对应的原始值
        target_bytes = target.to_bytes(8, byteorder='little', signed=True)
        speed_value = int(self.low_speed * 100)
        speed_bytes = speed_value.to_bytes(4, byteorder='little', signed=False)
        
        self._send_command(0xA4, target_bytes + speed_bytes)
        
        start_time = time.time()
        while time.time() - start_time < 300:
            current = self.get_current_angle()
            if current is not None and abs(current - 360) <= 0.1:
                print("已完成360度旋转")
                return True
            time.sleep(0.5)
        print("旋转超时")
        return False

    def close(self):
        # 共享串口由外部统一关闭，此处不重复关闭
        pass


class LE100485Sensor:
    def __init__(self, serial_port=None, port='COM3', baudrate=115200, sensor_address=2):
        """
        初始化传感器（支持外部传入串口实例）
        :param serial_port: 已创建的串口实例（RS485共享时使用）
        :param port: 串口端口（未传入serial_port时使用）
        :param baudrate: 波特率
        :param sensor_address: 传感器从机地址（区分设备的关键）
        """
        # 共享串口处理
        if serial_port:
            self.serial_port = serial_port
        else:
            self.serial_port = serial.Serial(
                port=port,
                baudrate=baudrate,
                bytesize=serial.EIGHTBITS,
                parity=serial.PARITY_NONE,
                stopbits=serial.STOPBITS_ONE,
                timeout=1
            )
        
        # 初始化Modbus-RTU通讯
        self.instrument = minimalmodbus.Instrument(
            port=self.serial_port,  # 使用共享串口
            slaveaddress=sensor_address
        )
        self.instrument.mode = minimalmodbus.MODE_RTU
        self.instrument.clear_buffers_before_each_transaction = True
        
        # 传感器参数
        self.distance_register = 0x003B
        self.resolution = 0.001  # 单位：mm

    def read_distance(self, register_address=None):
        if register_address is None:
            register_address = self.distance_register
        try:
            raw_data = self.instrument.read_register(
                registeraddress=register_address,
                functioncode=3,
                signed=False
            )
            return raw_data * self.resolution
        except Exception as e:
            print(f"传感器读取失败：{str(e)}")
            return None

    def close(self):
        # 共享串口由外部统一关闭，此处不重复关闭
        pass


class DataCollector:
    """数据采集器，负责同步采集角度和距离数据"""
    def __init__(self, motor, sensor):
        self.motor = motor
        self.sensor = sensor
        self.data = []
        self.running = False

    def _collect_loop(self, interval):
        """采集循环"""
        while self.running:
            angle = self.motor.get_current_angle()
            distance = self.sensor.read_distance()
            
            if angle is not None and distance is not None:
                normalized_angle = angle % 360  # 归一化到0-360度
                self.data.append((normalized_angle, distance))
                print(f"角度: {normalized_angle:.2f}°  距离: {distance:.5f}mm", end='\r')
            
            time.sleep(interval)

    def start(self, interval=0.01):
        """开始采集"""
        self.data = []
        self.running = True
        self.thread = threading.Thread(
            target=self._collect_loop,
            args=(interval,),
            daemon=True
        )
        self.thread.start()

    def stop(self):
        """停止采集"""
        self.running = False
        self.thread.join()

    def save_data(self, filename="measurement_data.txt"):
        """保存数据到文件"""
        with open(filename, 'w') as f:
            f.write("角度(度),距离(mm)\n")
            for angle, distance in self.data:
                f.write(f"{angle:.4f},{distance:.5f}\n")
        print(f"\n数据已保存到 {filename}，共 {len(self.data)} 条记录")